/******************************** ********************************
 * @file fire.cpp
 * @author L_Zero
 * @date 2025-8-17
 * @brief 发射机构驱动代码
 *
 * @note 实例创建即刻初始化，需要循环调用StateLoop()
 */
#include "fire.hpp"

namespace Fire_n {
    Fire_c *fire_ptr = nullptr;
    /* region ============================ 实例创建 ============================*/
    Fire_c *Fire_c::Get_InstancePtr()
    {
        static Fire_c _instance;
        fire_ptr = &_instance;
        return &_instance;
    }

    Fire_c::Fire_c()
    {
        this->is_loop = false;
        this->timer_instance = BSP_DWT_n::BSP_DWT_c::ECF_Get_DwtInstance();
        this->cmd_instance = CMD_n::CMD_c::Get_InstancePtr();
        this->vision = get_visiual_data();
        this->Friction_Init();
        this->Reloader_Init();
        this->is_loop = true;
    }
// endregion

    /* region ============================ 初始化 ============================*/
    void Fire_c::Friction_Init() {
        Motor_General_Def_n::Motor_Init_Config_s _config_friction = {
                .controller_param_init_config = {
                        .speed_PID = {
                                .Kp = FRICTION_WHEEL_DOWN_PID_KP,
                                .Ki = FRICTION_WHEEL_DOWN_PID_KI,
                                .Kd = FRICTION_WHEEL_DOWN_PID_KD,
                                .mode = Output_Limit | Integral_Limit | Deadzone,
                                .max_out = 9000,
                                .max_Ierror = FRICTION_WHEEL_DOWN_PID_I_MAX,
                                .deadband = 100.0f,
                                .deadband_max_out = 120.0f,
                                .deadband_center_out = -80.0f,
                        },
                },
                .controller_setting_init_config = {
                        .outer_loop_type       =  Motor_General_Def_n::SPEED_LOOP,
                        .close_loop_type       =  Motor_General_Def_n::SPEED_LOOP,
                        .motor_reverse_flag    =  Motor_General_Def_n::MOTOR_DIRECTION_REVERSE, // 目前三摩擦是反向的
                        .feedback_reverse_flag =  Motor_General_Def_n::FEEDBACK_DIRECTION_NORMAL,
                        // 反馈来源可设置为 MOTOR_FEED OTHER_FEED SPEED_APS ANGULAR_SPEED LINEAR_SPEED
                        .speed_feedback_source =  Motor_General_Def_n::MOTOR_FEED,
                },
                .motor_type = Motor_General_Def_n::M3508,
                .can_init_config = {
                        .can_handle = FRICTION_WHEEL_DOWN_USE_CAN,
                        .tx_id = FRICTION_WHEEL_DOWN_ID,
                }
        };
        this->friction_motor[FRICTION_WHEEL_DOWN_ID - 1] = new DJI_Motor_n::DJI_Motor_Instance(_config_friction);

        _config_friction.controller_param_init_config.speed_PID.Kp = FRICTION_WHEEL_L_PID_KP;
        _config_friction.controller_param_init_config.speed_PID.Ki = FRICTION_WHEEL_L_PID_KI;
        _config_friction.controller_param_init_config.speed_PID.Kd = FRICTION_WHEEL_L_PID_KD;
        _config_friction.controller_param_init_config.speed_PID.max_Ierror = FRICTION_WHEEL_L_PID_I_MAX;
        _config_friction.can_init_config.tx_id = FRICTION_WHEEL_L_ID;
        _config_friction.can_init_config.can_handle = FRICTION_WHEEL_L_USE_CAN;
        this->friction_motor[FRICTION_WHEEL_L_ID - 1] = new DJI_Motor_n::DJI_Motor_Instance(_config_friction);

        _config_friction.controller_param_init_config.speed_PID.Kp = FRICTION_WHEEL_R_PID_KP;
        _config_friction.controller_param_init_config.speed_PID.Ki = FRICTION_WHEEL_R_PID_KI;
        _config_friction.controller_param_init_config.speed_PID.Kd = FRICTION_WHEEL_R_PID_KD;
        _config_friction.controller_param_init_config.speed_PID.max_Ierror = FRICTION_WHEEL_R_PID_I_MAX;
        _config_friction.can_init_config.can_handle = FRICTION_WHEEL_R_USE_CAN;
        _config_friction.can_init_config.tx_id = FRICTION_WHEEL_R_ID;
        this->friction_motor[FRICTION_WHEEL_R_ID - 1] = new DJI_Motor_n::DJI_Motor_Instance(_config_friction);
    }

    void Fire_c::Reloader_Init() {
        Motor_General_Def_n::Motor_Init_Config_s _config_reloader = {
                .controller_param_init_config = {
                        .speed_PID = {
                                .Kp = RELOADER_MOTOR_PID_S_KP,
                                .Ki = RELOADER_MOTOR_PID_S_KI,
                                .Kd = RELOADER_MOTOR_PID_S_KD,
                                .ActualValueSource = nullptr,
                                .mode = Output_Limit | Integral_Limit,
                                .max_out = 9000,
                                .max_Ierror = 3000,
                                .deadband = 0.3,
                                // .stepIn = 3000,
                        },
                        .angle_PID = {
                                .Kp = RELOADER_MOTOR_PID_A_KP,
                                .Ki = RELOADER_MOTOR_PID_A_KI,
                                .Kd = RELOADER_MOTOR_PID_A_KD,
                                .ActualValueSource = nullptr,
                                .mode = Integral_Limit,
                                .max_out = 9000,
                                .max_Ierror = 3000,
                                .deadband = 0.3,
                                // .stepIn = 1000,
                        },
                },
                .controller_setting_init_config = {
                        .outer_loop_type = Motor_General_Def_n::SPEED_LOOP, // 使用速度环
                        .close_loop_type = Motor_General_Def_n::SPEED_LOOP,
                        .motor_reverse_flag = Motor_General_Def_n::MOTOR_DIRECTION_NORMAL,
                        .feedback_reverse_flag = Motor_General_Def_n::FEEDBACK_DIRECTION_NORMAL,
                        .angle_feedback_source = Motor_General_Def_n::MOTOR_FEED,
                        .speed_feedback_source = Motor_General_Def_n::MOTOR_FEED, // 电机反馈
                },
                .motor_type = Motor_General_Def_n::M2006, // 2006电机
                .can_init_config = {
                        .can_handle = &hcan2,       // 使用can2
                        .tx_id = RELOADER_MOTOR_ID,
                },
                .zero_offset = 0,
        }; // 初始位置为0
        this->reloader_motor = new DJI_Motor_n::DJI_Motor_Instance(_config_reloader);
    }
// endregion

    /* region ============================ 功能函数 ============================*/
    inline void Fire_c::Friction_Enable()
    {
        for (uint8_t i = 0; i < FRICTION_MOTOR_NUMS; i++)
        {
            if (/*!this->friction_motor[i]->is_lost_dji &&*/ this->friction_motor[i]->MotorMeasure.measure.init_flag)
            {
                this->friction_motor[i]->DJIMotorEnable();
            }

        }
    }

    inline void Fire_c::Friction_Disable()
    {
        for (uint8_t i = 0; i < FRICTION_MOTOR_NUMS; i++)
        {
            this->friction_motor[i]->DJIMotorStop();
        }
    }

    inline void Fire_c::Friction_Stop()
    {
        for (uint8_t i = 0; i < FRICTION_MOTOR_NUMS; i++)
        {
            this->friction_motor[i]->DJIMotorSetRef(0);
        }
    }

    inline void Fire_c::Friction_UpdateSpeed()
    {
        if (this->shoot_speed > MAX_SHOOT_SPEED)
        {
            this->shoot_speed = MAX_SHOOT_SPEED;
        }
        else if (this->shoot_speed < 0)
        {
            this->shoot_speed = 0;
        }
        for (uint8_t i = 0; i < FRICTION_MOTOR_NUMS; i++)
        {
            this->friction_motor[i]->DJIMotorSetRef(shoot_speed);
        }
    }

    inline void Fire_c::Reloader_Enable()
    {
        if (!this->reloader_motor->is_lost_dji && this->reloader_motor->MotorMeasure.measure.init_flag)
        {
            this->reloader_motor->DJIMotorEnable();
        }
    }

    inline void Fire_c::Reloader_Disable()
    {
        this->reloader_motor->DJIMotorStop();
    }

    inline void Fire_c::Reloader_Stop()
    {
        this->reloader_motor->motor_settings.close_loop_type = Motor_General_Def_n::SPEED_LOOP;
        this->reloader_motor->motor_settings.outer_loop_type = Motor_General_Def_n::SPEED_LOOP;
        this->reloader_motor->DJIMotorSetRef(0);
    }

    inline void Fire_c::Reloader_Clear()
    {
        this->reloader_motor->motor_settings.close_loop_type = Motor_General_Def_n::SPEED_LOOP;
        this->reloader_motor->motor_settings.outer_loop_type = Motor_General_Def_n::SPEED_LOOP;
        this->reloader_motor->DJIMotorSetRef(-4500);
    }

    inline void Fire_c::Reloader_StuckBack()
    {
        this->reloader_motor->motor_settings.close_loop_type = Motor_General_Def_n::OPEN_LOOP;
        this->reloader_motor->motor_settings.outer_loop_type = Motor_General_Def_n::OPEN_LOOP;
        this->reloader_motor->DJIMotorSetRef(-1000);
    }

    inline bool Fire_c::Check_Stuck()
    {
        return (abs(this->reloader_motor->MotorMeasure.measure.feedback_real_current) > 7900);
    }

    // 指定弹数的发射
    void Fire_c::DoShoot(uint8_t num, float freq)
    {
        static float Last_Shoot_Time = 0;
        // 到位检测
        if (this->reloader_motor->MotorMeasure.measure.record_ecd >= RELOADER_ENCODER_TO_SHOOT1 * num)
        {
            this->reloader_motor->motor_settings.close_loop_type = Motor_General_Def_n::SPEED_LOOP;
            this->reloader_motor->motor_settings.outer_loop_type = Motor_General_Def_n::SPEED_LOOP;
            this->reloader_motor->DJIMotorSetRef(0);
        }
        if (this->timer_instance->ECF_DWT_GetTimeline_s() - Last_Shoot_Time > (1.0f / freq))
        {
            this->reloader_motor->MotorMeasure.measure.record_ecd = 0;
            Last_Shoot_Time = this->timer_instance->ECF_DWT_GetTimeline_s();
            this->reloader_motor->motor_settings.close_loop_type = Motor_General_Def_n::ANGLE_AND_SPEED_LOOP;
            this->reloader_motor->motor_settings.outer_loop_type = Motor_General_Def_n::ANGLE_LOOP;
            this->reloader_motor->DJIMotorSetRef(
                    this->reloader_motor->MotorMeasure.measure.feedback_ecd + num * RELOADER_ENCODER_TO_SHOOT1);
        }
    }

    // 连发
    inline void Fire_c::DoShoot()
    {
        this->reloader_motor->motor_settings.close_loop_type = Motor_General_Def_n::SPEED_LOOP;
        this->reloader_motor->motor_settings.outer_loop_type = Motor_General_Def_n::SPEED_LOOP;
        this->reloader_motor->DJIMotorSetRef(this->shoot_freq * 60 * RELOADER_GEAR_RATIO / RELOADER_ROUND_BULLETS);
    }

    inline void Fire_c::Friction_AddSpeed(float val)
    {
        this->shoot_speed += val;
        if (this->shoot_speed > MAX_SHOOT_SPEED)
        {
            this->shoot_speed = MAX_SHOOT_SPEED;
        }
        else if (this->shoot_speed < MIN_SHOOT_SPEED)
        {
            this->shoot_speed = MIN_SHOOT_SPEED;
        }
    }

    inline void Fire_c::Reloader_AddFreq(float val)
    {
        this->shoot_freq += val;
        if (this->shoot_freq > MAX_SHOOT_FREQ)
        {
            this->shoot_freq = MAX_SHOOT_FREQ;
        }
        else if (this->shoot_freq < MIN_SHOOT_FREQ)
        {
            this->shoot_freq = MIN_SHOOT_FREQ;
        }

    }

    /**
     * @brief DT16控制下改变射击参数
     * */
    void Fire_c::Change_ShootVal_DT16()
    {
        /*
         * 左摇杆左上角且右拨杆居中：加弹速
         * 左摇杆左下角且右拨杆居中：减弹速
         * 左摇杆左上角且右拨杆上拨：加弹频
         * 左摇杆左下角且右拨杆上拨：减弹频
         */
        if (this->cmd_instance->Get_RC_LJoyLRValue() < -640)
        {
            if (this->cmd_instance->Get_RC_LJoyUDValue() > 640)
            {
                if (this->cmd_instance->Get_RC_SW2State() == CMD_n::CMD_MIDDLE)
                {
                    this->Friction_AddSpeed(SPEED_SENSOR_RC);
                }
                else if (this->cmd_instance->Get_RC_SW2State() == CMD_n::CMD_HIGH)
                {
                    this->Reloader_AddFreq(FREQ_SENSOR_RC);
                }
            }
            else if (this->cmd_instance->Get_RC_LJoyUDValue() < -640)
            {
                if (this->cmd_instance->Get_RC_SW2State() == CMD_n::CMD_MIDDLE)
                {
                    this->Friction_AddSpeed(-SPEED_SENSOR_RC);
                }
                else if (this->cmd_instance->Get_RC_SW2State() == CMD_n::CMD_HIGH)
                {
                    this->Reloader_AddFreq(-FREQ_SENSOR_RC);
                }
            }
        }
    }

    /**
     * @brief 图传控制下改变射击参数
     * */
    void Fire_c::Change_ShootVal_TC() {
        /** (O,o)! 暂定，应该要修改
         * C：弹速RPM逻辑
         * V：自瞄pitch补偿逻辑(未实现)
         * G：弹频逻辑
         * 鼠标滚轮：逻辑加减
         * WS：逻辑大幅加减
         * AD：逻辑小幅定额加减
         */
        if (this->cmd_instance->Check_TC_KeyPress('C', fire_ptr->cmd_instance->TC_cmd->kb.bit.C))
        {
            Friction_AddSpeed(this->cmd_instance->Get_TC_MouseZValue() * SPEED_SENSOR_TC * 0.001f);
            Friction_AddSpeed(this->cmd_instance->Check_TC_KeyPress('W', fire_ptr->cmd_instance->TC_cmd->kb.bit.W) *
                              SPEED_SENSOR_TC);
            Friction_AddSpeed(-this->cmd_instance->Check_TC_KeyPress('S', fire_ptr->cmd_instance->TC_cmd->kb.bit.S) *
                              SPEED_SENSOR_TC);
            Friction_AddSpeed(+this->cmd_instance->Check_TC_KeyPress('D', fire_ptr->cmd_instance->TC_cmd->kb.bit.D) *
                              SPEED_SENSOR_TC * 0.01f);
            Friction_AddSpeed(-this->cmd_instance->Check_TC_KeyPress('A', fire_ptr->cmd_instance->TC_cmd->kb.bit.A) *
                              SPEED_SENSOR_TC * 0.01f);
        }
        if (this->cmd_instance->Check_TC_KeyPress('G', fire_ptr->cmd_instance->TC_cmd->kb.bit.G))
        {
            Reloader_AddFreq(this->cmd_instance->Get_TC_MouseZValue() * FREQ_SENSOR_TC * 0.001f);
            Reloader_AddFreq(this->cmd_instance->Check_TC_KeyPress('W', fire_ptr->cmd_instance->TC_cmd->kb.bit.W) *
                             FREQ_SENSOR_TC);
            Reloader_AddFreq(-this->cmd_instance->Check_TC_KeyPress('S', fire_ptr->cmd_instance->TC_cmd->kb.bit.S) *
                             FREQ_SENSOR_TC);
            Reloader_AddFreq(+this->cmd_instance->Check_TC_KeyPress('D', fire_ptr->cmd_instance->TC_cmd->kb.bit.D) *
                             FREQ_SENSOR_TC * 0.01f);
            Reloader_AddFreq(-this->cmd_instance->Check_TC_KeyPress('A', fire_ptr->cmd_instance->TC_cmd->kb.bit.A) *
                             FREQ_SENSOR_TC * 0.01f);
        }
    }
// endregion

    /*============================ 状态机 ============================*/
    void Fire_c::ChangeState(Firc_State_e new_state)
    {
        this->is_loop = false; // 考虑到外部调用该函数
        this->StateExit();
        this->current_state = new_state;
        this->StateStart();
        this->is_loop = true;
    }

    void Fire_c::StateStart()
    {
        switch (this->current_state)
        {
            case Disable:
                this->timer_delta_t = 0.0f;
                this->timer_instance->ECF_DWT_GetDeltaT(&this->timer_cnt);
                this->Friction_Stop();
                this->Reloader_Stop();
                break;
            case Enable:
                this->Friction_Stop();
                this->Reloader_Stop();
                break;
            case Ready:
                this->Reloader_Stop();
                break;
            case OneShoot:
                this->timer_delta_t = 0.0f;
                this->timer_instance->ECF_DWT_GetDeltaT(&this->timer_cnt);
                break;
            case StartShoot:
                break;
            case Stuck:
                this->timer_delta_t = 0.0f;
                this->timer_instance->ECF_DWT_GetDeltaT(&this->timer_cnt);
                break;
            default:
                break;
        }
    }

    void Fire_c::StateExit()
    {
        switch (this->current_state)
        {
            case Disable:
                break;
            case Enable:
                this->is_triggers_locked = true;
                break;
            case Ready:
                this->is_triggers_locked = true;
                break;
            case OneShoot:
                break;
            case StartShoot:
                break;
            case Stuck:
                break;
            default:
                break;
        }
    }

    void StateLoop()
    {
        // 确定启动
        if (fire_ptr == nullptr)return;
        if (!fire_ptr->is_loop)return;

        // 检查连接
        CMD_n::CMD_ConnectState_e _connect_state = fire_ptr->cmd_instance->connect_state;
        if (_connect_state == CMD_n::NOT_CONNECT)
        {
            fire_ptr->is_triggers_locked = false;
            fire_ptr->ChangeState(Disable);
            fire_ptr->Friction_Disable();
            fire_ptr->Reloader_Disable();
            return;
        }

        // 状态机
        switch (fire_ptr->current_state) // (O,o)! 可读性有点差，可以把功能封装成函数
        {
            case Disable: // region Disable
                /* * * * * * * * * * * * * * * * * * * ctrl with DR16
                 * 计时器未到指定时间，摩擦轮、拨弹盘(速控)使能
                 * 计时器未到指定时间，计时器计时
                 * 计时器到指定时间，摩擦轮、拨弹盘失能
                 * 检测 拨杆 拨中/上拨 是否进入Enable状态
                 * * * * * * * * * * * * * * * * * * * ctrl with TC
                 * 检测到图传链接，直接进入Enable状态
                 */
                if (_connect_state == CMD_n::TC_CMD)
                {
                    fire_ptr->is_triggers_locked = false;
                    fire_ptr->ChangeState(Enable);
                    return;
                }
                if (fire_ptr->timer_delta_t < FIRE_DISABLE_TIME)
                {
                    fire_ptr->Friction_Enable();
                    fire_ptr->Reloader_Enable();
                    fire_ptr->timer_delta_t += fire_ptr->timer_instance->ECF_DWT_GetDeltaT(&fire_ptr->timer_cnt);
                }
                else
                {
                    fire_ptr->Friction_Disable();
                    fire_ptr->Reloader_Disable();
                }
                if (fire_ptr->cmd_instance->Get_RC_SW2State() >= CMD_n::CMD_MIDDLE)
                {
                    fire_ptr->ChangeState(Enable);
                    return;
                }
                break;
                // endregion
            case Enable: // region Enable
                /* * * * * * * * * * * * * * * * * * * * ctrl with DR16
                 * 摩擦轮、拨弹盘使能
                 * 检测 拨杆 下拨 是否进入Disable状态
                 * 锁定标志位1 检测 棘轮 居中 锁定标志位置0
                 * 锁定标志位0 检测 棘轮 上拨一点 是否进入Ready状态
                 * 检测 棘轮 上拨到底 是:拨弹盘反转给速度（退弹） 否:拨弹盘速度给0
                 * * * * * * * * * * * * * * * * * * * * ctrl with TC
                 * 摩擦轮、拨弹盘使能
                 * 锁定标志位1 检测 R键 抬起 锁定标志位置0
                 * 锁定标志位0 检测 R键 抬起 进入Ready状态
                 */
                fire_ptr->Friction_Enable();
                fire_ptr->Reloader_Enable();
                fire_ptr->Friction_Stop();
                fire_ptr->Reloader_Stop();
                if (_connect_state == CMD_n::TC_CMD) // 图传控制
                {
                    fire_ptr->Change_ShootVal_TC();
                    if (!fire_ptr->is_triggers_locked) // 防止重复触发
                    {
                        if (fire_ptr->cmd_instance->Check_TC_KeyUp('R', fire_ptr->cmd_instance->TC_cmd->kb.bit.R))
                        {
                            fire_ptr->ChangeState(Ready);
                            return;
                        }
                    }
                    else if (fire_ptr->cmd_instance->Check_TC_KeyUp('R', fire_ptr->cmd_instance->TC_cmd->kb.bit.R))
                    {
                        fire_ptr->is_triggers_locked = false;
                    }
                }
                else // DR16控制
                {
                    fire_ptr->Change_ShootVal_DT16();
                    if (fire_ptr->cmd_instance->Get_RC_SW2State() <= CMD_n::CMD_LOW) // 右拨杆下拨失能
                    {
                        fire_ptr->ChangeState(Disable);
                        return;
                    }
                    if (fire_ptr->cmd_instance->Get_RC_RatchetState() == CMD_n::CMD_HIGH) // 棘轮上拨到底退弹
                    {
                        fire_ptr->Reloader_Clear();
                    }
                    if (!fire_ptr->is_triggers_locked) // 防止重复触发
                    {
                        if (fire_ptr->cmd_instance->Get_RC_RatchetState() >= CMD_n::CMD_MIDDLE_HIGH) // 棘轮上拨一点进入Ready（开摩擦轮
                        {
                            fire_ptr->ChangeState(Ready);
                            return;
                        }
                    }
                    else
                    {
                        if (fire_ptr->cmd_instance->Get_RC_RatchetState() <= CMD_n::CMD_MIDDLE)
                        {
                            fire_ptr->is_triggers_locked = false;
                        }
                    }
                }
                break;
                // endregion
            case Ready: // region Ready
                /* * * * * * * * * * * * * * * * * * * * ctrl with DR16
                 * 摩擦轮给指定速度
                 * 摩擦轮、拨弹盘使能
                 * 检测 拨杆 下拨 是否进入Disable状态
                 * 棘轮居中标志位为0 检测 棘轮 居中 居中标志位置1
                 * 棘轮居中标志位为1 检测 棘轮 上拨一点 是否进入Enable状态
                 * 检测 棘轮 上拨到底 是:拨弹盘反转给速度（退弹） 否:拨弹盘速度给0
                 * 检测 棘轮 下拨 拨杆 居中 是否进入OneShoot状态
                 * 检测 棘轮 下拨 拨杆 上拨 是否进入StartShoot状态
                 * 检测 左摇杆左上角 拨杆居中 加弹速
                 * 检测 左摇杆左上角 拨杆上拨 加弹频
                 * 检测 左摇杆左下角 拨杆居中 减弹速
                 * 检测 左摇杆左下角 拨杆上拨 减弹频
                 * * * * * * * * * * * * * * * * * * * * ctrl with TC
                 * 摩擦轮给指定速度
                 * 摩擦轮、拨弹盘使能
                 * 检测 R键 按下 进入Enable状态
                 * 检测 左键 按压 且弹频率<=1 进入OneShoot状态
                 * 检测 左键 按压 且弹频率 >1 进入StartShoot状态
                 */
                fire_ptr->Friction_Enable();
                fire_ptr->Reloader_Enable();
                fire_ptr->Friction_UpdateSpeed();
                fire_ptr->Reloader_Stop();
                if (_connect_state == CMD_n::TC_CMD) // 图传控制
                {
                    fire_ptr->Change_ShootVal_TC();
                    if (fire_ptr->cmd_instance->Check_TC_KeyDown('R', fire_ptr->cmd_instance->TC_cmd->kb.bit.R))
                    {
                        fire_ptr->ChangeState(Enable); // R键开关摩擦轮
                        return;
                    }
                    if (fire_ptr->cmd_instance->Check_TC_KeyPress('l', fire_ptr->cmd_instance->TC_cmd->mouse.press_l))
                    {
                        if(fire_ptr->shoot_freq <= 1) // 左键逻辑，低频单发
                        {
                            fire_ptr->ChangeState(OneShoot);
                            return;
                        }
                        else
                        {
                            fire_ptr->ChangeState(StartShoot);
                            return;
                        }
                    }
                }
                else // DR16控制
                {
                    fire_ptr->Change_ShootVal_DT16();
                    if (fire_ptr->cmd_instance->Get_RC_SW2State() <= CMD_n::CMD_LOW)
                    {
                        fire_ptr->ChangeState(Disable);
                        return;
                    }
                    if (fire_ptr->cmd_instance->Get_RC_RatchetState() == CMD_n::CMD_HIGH)
                    {
                        fire_ptr->Reloader_Clear();
                    }
                    if (!fire_ptr->is_triggers_locked)
                    {
                        if (fire_ptr->cmd_instance->Get_RC_RatchetState() >= CMD_n::CMD_MIDDLE_HIGH)
                        {
                            fire_ptr->ChangeState(Enable);
                            return;
                        }
                    }
                    else
                    {
                        if (fire_ptr->cmd_instance->Get_RC_RatchetState() <= CMD_n::CMD_MIDDLE)
                        {
                            fire_ptr->is_triggers_locked = false;
                        }
                    }
                    if (fire_ptr->cmd_instance->Get_RC_RatchetState() == CMD_n::CMD_LOW)
                    {
                        if (fire_ptr->cmd_instance->Get_RC_SW2State() == CMD_n::CMD_MIDDLE)
                        {
                            fire_ptr->ChangeState(OneShoot);
                            return;
                        }
                        if (fire_ptr->cmd_instance->Get_RC_SW2State() == CMD_n::CMD_HIGH)
                        {
                            fire_ptr->ChangeState(StartShoot);
                            return;
                        }
                    }
                }
                break;
                // endregion
            case OneShoot: // region OneShoot
                /* * * * * * * * * * * * * * * * * * * * * ctrl with DR16
                 * 摩擦轮给指定速度
                 * 摩擦轮、拨弹盘使能
                 * 检测 调用卡弹检测函数 是否进入Stuck状态
                 * 检测 拨杆 下拨 是否进入Disable状态
                 * 计时器没到单发时长，计时器计时
                 * 计时器没到单发时长，调用发射函数
                 * 计时器到单发时长，检测 棘轮 居中以上 进入Ready状态
                 * * * * * * * * * * * * * * * * * * * * * ctrl with TC
                 * 摩擦轮给指定速度
                 * 摩擦轮、拨弹盘使能
                 * 检测 调用卡弹检测函数 是否进入Stuck状态
                 * 计时器没到单发时长，计时器计时
                 * 计时器没到单发时长，调用发射函数
                 * 计时器到单发时长，回到Ready状态
                 */
                fire_ptr->Friction_Enable();
                fire_ptr->Reloader_Enable();
                fire_ptr->Friction_UpdateSpeed();
                if (_connect_state == CMD_n::DR16_CMD && fire_ptr->cmd_instance->Get_RC_SW2State() <= CMD_n::CMD_LOW)
                {
                    fire_ptr->ChangeState(Disable);
                    return;
                }
                if (fire_ptr->Check_Stuck())
                {
                    fire_ptr->ChangeState(Stuck); // (O,o)? 检测到卡弹会出现连射两弹的情况吗?
                    return;
                }
                if (fire_ptr->timer_delta_t < FIRE_ONE_SHOT_TIME)
                {
                    fire_ptr->timer_delta_t += fire_ptr->timer_instance->ECF_DWT_GetDeltaT(&fire_ptr->timer_cnt);
                    fire_ptr->DoShoot(1, 2);
                }
                else
                {
                    if (_connect_state == CMD_n::TC_CMD)
                    {
                        fire_ptr->ChangeState(Ready);
                        return;
                    }
                    else if (fire_ptr->cmd_instance->Get_RC_SW2State() >= CMD_n::CMD_MIDDLE)
                    {
                        fire_ptr->ChangeState(Ready);
                        return;
                    }
                }
                break;
                // endregion
            case StartShoot: // region StartShoot
                /* * * * * * * * * * * * * * * * * * * * * * ctrl with DR16
                 * 摩擦轮给指定速度
                 * 摩擦轮、拨弹盘使能
                 * 调用发射函数，若 左拨杆 上拨，检测视觉通讯是否可发射，再调用发射函数
                 * 检测 调用卡弹检测函数 是否进入Stuck状态
                 * 检测 拨杆 下拨 是否进入Disable状态
                 * 检测 棘轮 居中以上 进入Ready状态
                 * * * * * * * * * * * * * * * * * * * * * * ctrl with TC
                 * 摩擦轮给指定速度
                 * 摩擦轮、拨弹盘使能
                 * 调用发射函数，若 右键 按压，检测视觉通讯是否可发射，再调用发射函数
                 * 检测 调用卡弹检测函数 是否进入Stuck状态
                 * 检测 左键 非按压 回到Ready状态
                 */
                fire_ptr->Friction_Enable();
                fire_ptr->Reloader_Enable();
                fire_ptr->Friction_UpdateSpeed();
                if (_connect_state == CMD_n::TC_CMD)
                {
                    if (!fire_ptr->cmd_instance->Check_TC_KeyPress('l', fire_ptr->cmd_instance->TC_cmd->mouse.press_l))
                    {
                        fire_ptr->ChangeState(Ready);
                        return;
                    }
                    if (fire_ptr->cmd_instance->Check_TC_KeyPress('r', fire_ptr->cmd_instance->TC_cmd->mouse.press_r))
                    {
                        if(fire_ptr->vision->rx_data.fire_flag)
                        {
                            fire_ptr->DoShoot();
                        }
                    }
                    else
                    {
                        fire_ptr->DoShoot();
                    }
                }
                else
                {
                    if (fire_ptr->cmd_instance->Get_RC_SW2State() <= CMD_n::CMD_LOW)
                    {
                        fire_ptr->ChangeState(Disable);
                        return;
                    }
                    if(fire_ptr->cmd_instance->Get_RC_RatchetState() >= CMD_n::CMD_MIDDLE)
                    {
                        fire_ptr->ChangeState(Ready);
                        return;
                    }
                    fire_ptr->DoShoot();
                }
                if (fire_ptr->Check_Stuck())
                {
                    fire_ptr->ChangeState(Stuck);
                    return;
                }
                break;
                // endregion
            case Stuck: // region Stuck
                /*
                 * 摩擦轮给指定速度
                 * 摩擦轮、拨弹盘使能
                 * 计时器没到回退时长，计时器计时
                 * 计时器没到回退时长，拨弹盘反转
                 * 计时器到回退时长，进入Ready状态
                 */
                fire_ptr->Friction_Enable();
                fire_ptr->Reloader_Enable();
                fire_ptr->Friction_UpdateSpeed();
                if (fire_ptr->timer_delta_t < FIRE_STUCK_TIME)
                {
                    fire_ptr->timer_delta_t += fire_ptr->timer_instance->ECF_DWT_GetDeltaT(&fire_ptr->timer_cnt);
                    fire_ptr->Reloader_StuckBack();
                }
                else
                {
                    fire_ptr->ChangeState(Ready);
                    return;
                }
                break;
                // endregion
            default:
                break;
        }
    }
}